#include "main.h"
#include "app_motion.h"
#include <math.h>

#define REDUCTION_RATIO (21.3f)
#define LINE_COUNT (22.0f)
#define DETECH_MAX_RAW_SPEED (2700.0f)
#define GPIO_STATE_INVERT(a, x) (GPIO_PinState)(a ? !x : x)

static const char *TAG = "Motor";

std::map<uint16_t, Motor *> Motor::pin_to_motor_map;

Motor::Motor(pwm_t pwm, gpio_t pinA, gpio_t pinB, gpio_t enc_pinA, gpio_t enc_pinB, IRQn_Type enc_irq, bool isInv)
    : pwm(pwm),
      pinA(pinA),
      pinB(pinB),
      enc_pinA(enc_pinA),
      enc_pinB(enc_pinB),
      enc_irq(enc_irq),
      isInv(isInv)
// ,pid(1.5, 1, 0.01, 0.8)
{

    HAL_TIM_Base_Start_IT(pwm.tim);
    HAL_TIM_PWM_Start(pwm.tim, pwm.channel);
    pin_to_motor_map[enc_pinA.pin] = this;
}
Motor::~Motor()
{
    pin_to_motor_map.erase(this->enc_pinA.pin);
}

void Motor::set_torque(float torque)
{
    if (torque < 0)
    {
        torque = -torque;
        set_moode(MODE_BACKWARD);
    }
    else
    {
        set_moode(MODE_FORWARD);
    }

    if (torque > 1)
        torque = 1;

    uint16_t pulse_us = (uint16_t)(torque * this->pwm.tim->Init.Period);
    __HAL_TIM_SET_COMPARE(this->pwm.tim, this->pwm.channel, pulse_us);
}
void Motor::set_moode(Motor_Mode mode)
{
    switch (mode)
    {
    case MODE_FORWARD:
        HAL_GPIO_WritePin(pinA.port, pinA.pin, GPIO_STATE_INVERT(isInv, GPIO_PIN_SET));
        HAL_GPIO_WritePin(pinB.port, pinB.pin, GPIO_STATE_INVERT(isInv, GPIO_PIN_RESET));
        break;
    case MODE_BACKWARD:
        HAL_GPIO_WritePin(pinA.port, pinA.pin, GPIO_STATE_INVERT(isInv, GPIO_PIN_RESET));
        HAL_GPIO_WritePin(pinB.port, pinB.pin, GPIO_STATE_INVERT(isInv, GPIO_PIN_SET));
        break;
    case MODE_NETURAL:
        HAL_GPIO_WritePin(pinA.port, pinA.pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(pinB.port, pinB.pin, GPIO_PIN_RESET);
        break;
    case MODE_STOP:
        HAL_GPIO_WritePin(pinA.port, pinA.pin, GPIO_PIN_SET);
        HAL_GPIO_WritePin(pinB.port, pinB.pin, GPIO_PIN_SET);
        break;
    default:
        break;
    }
}
float Motor::get_speed()
{
    float raw = 0.0, T = 0.0;
    HAL_NVIC_DisableIRQ(this->enc_irq);

    uint32_t cur_us = get_us();

#define _max(a, b) ((a) > (b) ? (a) : (b))

    T = _max(this->intrupt_us - this->last_intrupt_us, cur_us - this->intrupt_us) / 1.0e6f;
    raw = 360.0f * direction / T / REDUCTION_RATIO / LINE_COUNT;

    HAL_NVIC_EnableIRQ(this->enc_irq);

    raw /= DETECH_MAX_RAW_SPEED;

    const float alpha = 0.15; /*越小越平滑，响应越慢（0.1~0.3都可以试试）*/

    if (fabsf(raw - last_valid_speed) > 0.1f and fabsf(last_valid_speed) > 0.03f and fabsf(raw) < 0.01)
    {
        return last_valid_speed;
    }

    last_valid_speed = alpha * raw + (1.0f - alpha) * last_valid_speed;

    return last_valid_speed;
}

void Motor::handle_enc_a_interrupt()
{
    // printf("handle_enc_a_interrupt at %d\n",enc_pinA);
    if (GPIO_STATE_INVERT(isInv, GPIO_PIN_RESET) == HAL_GPIO_ReadPin(enc_pinB.port, this->enc_pinB.pin))
    {
        this->direction = 1;
    }
    else
    {
        this->direction = -1;
    }
    this->last_intrupt_us = this->intrupt_us;
    this->intrupt_us = get_us();
}

void Motor::dispatch_interrupt(uint16_t GPIO_Pin)
{

    auto motor = pin_to_motor_map.find(GPIO_Pin);
    if (motor != pin_to_motor_map.end())
    {
        motor->second->handle_enc_a_interrupt();
    }
}

void Motor::poll(float dt)
{
    float cur_speed = get_speed();
    float error = target_speed - cur_speed;
    // error=-error;
    float output = pid->update(error, dt) + target_speed;

    set_torque(output);

    printf( "%.3f,%+.3f,%.3f", output, error, cur_speed);
}

extern "C" void user_motor_gpio_inturrupt_handler(uint16_t GPIO_Pin)
{
    // printf("user_motor_gpio_inturrupt_handler %d\n", GPIO_Pin);
    Motor::dispatch_interrupt(GPIO_Pin);
}
